/*
    MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Accelerometer Example.
    Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
    GIT: https://github.com/jarzebski/Arduino-MPU6050
    Web: http://www.jarzebski.pl
    (c) 2014 by Korneliusz Jarzebski

    T-Block is adapted by Lewis He July 27,2020
*/

#include "config.h"

TTGOClass *watch;
MPU6050 *mpu;

void checkSettings()
{
    Serial.println();

    Serial.print(" * Sleep Mode:        ");
    Serial.println(mpu->getSleepEnabled() ? "Enabled" : "Disabled");

    Serial.print(" * Clock Source:      ");
    switch (mpu->getClockSource()) {
    case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
    case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
    case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
    case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
    case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
    }

    Serial.print(" * Gyroscope:         ");
    switch (mpu->getScale()) {
    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
    case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
    case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
    }

    Serial.print(" * Gyroscope offsets: ");
    Serial.print(mpu->getGyroOffsetX());
    Serial.print(" / ");
    Serial.print(mpu->getGyroOffsetY());
    Serial.print(" / ");
    Serial.println(mpu->getGyroOffsetZ());

    Serial.println();
}

void setup()
{
    Serial.begin(115200);

    watch = TTGOClass::getWatch();

    watch->begin();

    mpu = watch->mpu;

    // If you want, you can set gyroscope offsets
    // mpu->setGyroOffsetX(155);
    // mpu->setGyroOffsetY(15);
    // mpu->setGyroOffsetZ(15);

    // Calibrate gyroscope. The calibration must be at rest.
    // If you don't want calibrate, comment this line.
    mpu->calibrateGyro();

    // Set threshold sensivty. Default 3.
    // If you don't want use threshold, comment this line or set 0.
    mpu->setThreshold(3);

    checkSettings();
}

void loop()
{
    Vector rawGyro = mpu->readRawGyro();
    Vector normGyro = mpu->readNormalizeGyro();

    Serial.print(" Xraw = ");
    Serial.print(rawGyro.XAxis);
    Serial.print(" Yraw = ");
    Serial.print(rawGyro.YAxis);
    Serial.print(" Zraw = ");
    Serial.println(rawGyro.ZAxis);

    Serial.print(" Xnorm = ");
    Serial.print(normGyro.XAxis);
    Serial.print(" Ynorm = ");
    Serial.print(normGyro.YAxis);
    Serial.print(" Znorm = ");
    Serial.println(normGyro.ZAxis);

    delay(200);
}

